﻿/*******************************************************************************
 * @file nmea.c
 * @brief NMEA4.1 协议解析
 *******************************************************************************
*/

#include "nmea.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

/**
 * @brief 解析经纬度字符串为浮点数
 * @param str 输入的经纬度字符串
 * @param len 字符串长度
 * @return 转换后的浮点数表示的经纬度
 */
static float _ParseCoordinate(const char *str, int len)
{
    // 参数检查
    if(!str || (len < 9)) return 0.0f;

    // 提取度和分
    char deg_str[4] = { 0 };
    char min_str[10] = { 0 };
    strncpy(deg_str, str, len - 7);
    strncpy(min_str, str + len - 7, 7);
    // 转换为浮点数
    float degrees = atof(deg_str);
    float minutes = atof(min_str);

    return degrees + (minutes / 60.0f);
}

/**
 * @brief 计算 XOR 校验
 * @param nmea_sentence NMEA 语句
 * @param len NMEA 语句长度 (包含\r\n)
 * @return 校验结果，0表示校验失败，非0表示校验成功
 */
static uint8_t _XorCheck(const char *nmea_sentence, size_t len)
{
    // 参数检查
    if(!nmea_sentence || (len > strlen(nmea_sentence))) return 0;

    uint8_t xor_check = 0; // 初始化校验值为0
    const char *pos = NULL; // 用于存储 * 位置

    // 获取 * 位置，从后查找，最多查找到倒数第10个字符，提高效率
    for(pos = nmea_sentence + len - 3;
        (pos >= nmea_sentence + len - 10) && (pos > nmea_sentence);
        pos--)
    {
        if(*pos == '*') break;
    }

    // 如果没有找到 * 位置，返回0
    if(*pos != '*') return 0;

    // 计算校验值，从 $ 后一个字符开始，直到 * 前一个字符
    for(const char *p = nmea_sentence + 1; p < pos; p++)
    {
        xor_check ^= *p;
    }

    // 校验
    if(xor_check != (uint8_t)strtoul(pos + 1, NULL, 16)) return 0;

    return xor_check;
}

/**
 * @brief 预处理 NMEA 语句，包括帧头检查、长度计算、校验和内存分配
 * @param nmea_sentence NMEA 语句
 * @param header 语句帧头 e.g. GGA, GLL, GSA, GSV, RMC, VTG, ZDA
 * @param end 下一个语句的起始位置
 * @param sentence 用于存储有效数据的指针
 * @return 有效数据的指针，NULL表示处理失败
 */
static int _Preprocess(const char *nmea_sentence, const char *header, char **end)
{
    // 参数检查
    if(!nmea_sentence || !header) return NMEA_ERROR;

    size_t len = 0;

    // 检查帧头是否为 $--header 格式，-- 为任意字符，header 为指定的帧头
    if(nmea_sentence[0] != '$' || strlen(nmea_sentence) < strlen(header) + 3)
    {
        return NMEA_ERROR;
    }
    if(strncmp(nmea_sentence + 3, header, strlen(header)) != 0)
    {
        return NMEA_ERROR;
    }

    // 获取\r\n位置，并计算帧长度
    char *pos = strstr(nmea_sentence, "\r\n");
    if(!pos) return NMEA_ERROR;
    len = pos - nmea_sentence + 2; // 包含\r\n

    // 记录下一个语句的起始位置
    if(end) *end = pos + 2;

    // 计算 XOR 校验值
    if(!_XorCheck(nmea_sentence, len)) return NMEA_ERROR;

    return NMEA_OK;
}


/**
 * @brief 解析 GGA 语句
 * @param nmea_sentence NMEA 语句 
 * @param data 解析得到的数据
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseGGA(const char *nmea_sentence, NmeaGGA_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "GGA", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaGGA_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 逐字段解析
    // 解析时间
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    const char *next_token = strchr(token, ',');
    if(!next_token) return NMEA_ERROR;
    size_t len = next_token - token;
    strncpy(data->time, token, len > sizeof(data->time) - 1 ? sizeof(data->time) - 1 : len);
    data->time[sizeof(data->time) - 1] = '\0';
    token = next_token;

    // 解析纬度
    token++;
    next_token = strchr(token, ',');
    if(!next_token) return NMEA_ERROR;
    len = next_token - token;
    data->latitude = _ParseCoordinate(token, (int)len);
    token = next_token;

    // 解析纬度方向
    token++;
    if(*token == 'N' || *token == 'S')
        data->lat_dir = *token;

    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析经度
    token++;
    next_token = strchr(token, ',');
    if(!next_token) return NMEA_ERROR;
    len = next_token - token;
    data->longitude = _ParseCoordinate(token, (int)len);
    token = next_token;

    // 解析经度方向
    token++;
    if(*token == 'E' || *token == 'W')
        data->lon_dir = *token;

    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析 GPS 状态
    token++;
    data->gps_state = (uint8_t)atoi(token);
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析卫星数量
    token++;
    data->num_satellites = (uint8_t)atoi(token);
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析水平精度因子
    token++;
    data->hdop = atof(token);
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析海拔高度
    token++;
    data->altitude = atof(token);
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析大地水准面高度
    token++;
    data->geoid_sep = atof(token);
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析差分数据龄期
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        data->age = atof(token);
        token = next_token;
    }

    // 解析参考站 ID
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        data->ref_station_id = atof(token);
    }

    return NMEA_OK;
}

/**
 * @brief 解析 GLL 语句
 * @param nmea_sentence 输入的 NMEA GLL 语句字符串
 * @param data 用于存储解析结果的 NmeaGLL_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseGLL(const char *nmea_sentence, NmeaGLL_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "GLL", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaGLL_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 逐字段解析
    // 解析纬度
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    const char *next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        data->latitude = _ParseCoordinate(token, (int)len);
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析纬度方向
    token++;
    if(*token == 'N' || *token == 'S')
        data->lat_dir = *token;

    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析经度
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        data->longitude = _ParseCoordinate(token, (int)len);
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析经度方向
    token++;
    if(*token == 'E' || *token == 'W')
        data->lon_dir = *token;
    
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;

    // 解析时间
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        strncpy(data->time, token, len > sizeof(data->time) - 1 ? sizeof(data->time) - 1 : len);
        data->time[sizeof(data->time) - 1] = '\0';
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析状态
    token++;
    if(*token != ',')
        data->status = *token;

#if (NMEA_VERSION >= 23)
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    // 解析模式
    token++;
    if(*token != ',')
        data->mode = *token;
#endif

    return NMEA_OK;
}

/**
 * @brief 解析 GSA 语句
 * @param nmea_sentence 输入的 NMEA GST 语句字符串
 * @param data 用于存储解析结果的 NmeaGSA_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseGSA(const char *nmea_sentence, NmeaGSA_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "GSA", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    // 修正为清零 NmeaGSA_t 结构体
    memset(data, 0, sizeof(NmeaGSA_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 逐字段解析
    // 解析模式
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    if(*token!= ',')
        data->mode = *token;

    // 解析定位类型
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->fix_type = (uint8_t)atoi(token);

    // 解析 12 个 PRN 编号
    for(int i = 0; i < 12; i++)
    {
        token = strchr(token, ',');
        if(!token) break;
        token++;
        data->prn[i] = atoi(token);
    }

    // 解析 PDOP
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->pdop = atof(token);

    // 解析 HDOP
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->hdop = atof(token);

    // 解析 VDOP
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->vdop = atof(token);

#if (NMEA_VERSION >= 41)
    // 解析系统 ID
    token = strchr(token, ',');
    if(token)
    {
        token++;
        data->sys_id = atoi(token);
    }
#endif

    return NMEA_OK;
}

/**
 * @brief 解析 GSV 语句
 * @param nmea_sentence 输入的 NMEA GSV 语句字符串
 * @param data 用于存储解析结果的 NmeaGSV_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseGSV(const char *nmea_sentence, NmeaGSV_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "GSV", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaGSV_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 解析总消息数
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->num_messages = atoi(token);

    // 解析当前消息号
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->msg_number = atoi(token);

    // 解析可见卫星总数
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->num_satellites_in_view = atoi(token);

    // 解析卫星信息
    for(int i = 0; i < 4; i++)
    {
        token = strchr(token, ',');
        if(!token) break;
        token++;
        if(*token != '\0') data->satellite_prn[i] = atoi(token);

        token = strchr(token, ',');
        if(!token) break;
        token++;
        if(*token != '\0') data->elevation[i] = atoi(token);

        token = strchr(token, ',');
        if(!token) break;
        token++;
        if(*token != '\0') data->azimuth[i] = atoi(token);

        token = strchr(token, ',');
        if(!token) break;
        token++;
        if(*token != '\0') data->snr[i] = atoi(token);
    }

#if (NMEA_VERSION >= 41)
    // 解析GNSS信号ID
    token = strchr(token, ',');
    if(token)
    {
        token++;
        data->signal_id = atoi(token);
    }
#endif

    return NMEA_OK;
}

/**
 * @brief 解析 RMC 语句
 * @param nmea_sentence 输入的 NMEA RMC 语句字符串
 * @param data 用于存储解析结果的 NmeaRMC_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseRMC(const char *nmea_sentence, NmeaRMC_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "RMC", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaRMC_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 解析时间
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    const char *next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        strncpy(data->time, token, len > sizeof(data->time) - 1 ? sizeof(data->time) - 1 : len);
        data->time[sizeof(data->time) - 1] = '\0';
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析状态
    token++;
    if(*token!= ',')
        data->status = *token;

    // 解析纬度
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        data->latitude = _ParseCoordinate(token, (int)len);
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析纬度方向
    token++;
    if(*token == 'N' || *token == 'S')
        data->lat_dir = *token;

    // 解析经度
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        data->longitude = _ParseCoordinate(token, (int)len);
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析经度方向
    token++;
    if(*token == 'E' || *token == 'W')
        data->lon_dir = *token;

    // 解析速度
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->speed = atof(token);

    // 解析航向
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->course = atof(token);

    // 解析日期
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        strncpy(data->date, token, len > sizeof(data->date) - 1 ? sizeof(data->date) - 1 : len);
        data->date[sizeof(data->date) - 1] = '\0';
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析磁偏角
    token++;
    data->magnetic_variation = atof(token);

    // 解析磁偏角方向
    token = strchr(token, ',');
    if(token)
    {
        token++;
        if(*token!= ',')
            data->mag_var_dir = *token;
    }

#if (NMEA_VERSION >= 23)
    // 解析模式
    token = strchr(token, ',');
    if(token)
    {
        token++;
        if(*token!= ',')
            data->mode = *token;
    }
#endif

#if (NMEA_VERSION >= 41)
    // 解析导航状态
    token = strchr(token, ',');
    if(token)
    {
        token++;
        if(*token!= ',')
            data->nav_status = *token;
    }
#endif

    return NMEA_OK;
}

/**
 * @brief 解析 VTG 语句
 * @param nmea_sentence 输入的 NMEA VTG 语句字符串
 * @param data 用于存储解析结果的 NmeaVTG_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseVTG(const char *nmea_sentence, NmeaVTG_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "VTG", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaVTG_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 解析真航向
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->true_course = atof(token);

    // 跳过 T
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;

    // 解析磁航向
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->magnetic_course = atof(token);

    // 跳过 M
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;

    // 解析速度（节）
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->speed_knots = atof(token);

    // 跳过 N
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;

    // 解析速度（千米/小时）
    token = strchr(token, ',');
    if(token)
    {
        token++;
        data->speed_kmh = atof(token);
    }

    // 跳过 K
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;

#if (NMEA_VERSION >= 23)
    // 解析模式
    token = strchr(token, ',');
    if(token)
    {
        token++;
        if(*token!= ',')
            data->mode = *token;
    }
#endif

    return NMEA_OK;
}

/**
 * @brief 解析 ZDA 语句
 * @param nmea_sentence 输入的 NMEA ZDA 语句字符串
 * @param data 用于存储解析结果的 NmeaZDA_t 结构体指针
 * @param end 下一个语句的起始位置
 * @return 解析结果，NMEA_ERROR 表示解析失败，NMEA_OK 表示解析成功
 */
int Nmea_ParseZDA(const char *nmea_sentence, NmeaZDA_t *data, char **end)
{
    if(!nmea_sentence || !data) return NMEA_ERROR;

    char *_end = NULL;
    if(_Preprocess(nmea_sentence, "ZDA", &_end) != NMEA_OK) return NMEA_ERROR;
    if(end) *end = _end;

    memset(data, 0, sizeof(NmeaZDA_t));

    // 跳过帧头
    const char *token = nmea_sentence + 6;
    if(!token) return NMEA_ERROR;

    // 解析时间
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    const char *next_token = strchr(token, ',');
    if(next_token)
    {
        size_t len = next_token - token;
        strncpy(data->time, token, len > sizeof(data->time) - 1 ? sizeof(data->time) - 1 : len);
        data->time[sizeof(data->time) - 1] = '\0';
        token = next_token;
    }
    else
    {
        return NMEA_ERROR;
    }

    // 解析日
    token++;
    data->day = atoi(token);

    // 解析月
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->month = atoi(token);

    // 解析年
    token = strchr(token, ',');
    if(!token) return NMEA_ERROR;
    token++;
    data->year = atoi(token);

    return NMEA_OK;
}

/**
 * @brief 获取下一个 NMEA 语句
 * @param nmea_sentence 输入的 NMEA 语句字符串
 * @return 下一个语句的起始位置指针，如果没有下一个语句则返回 NULL
 */
const char *Nmea_NextSentence(const char *nmea_sentence)
{
    if(!nmea_sentence) return NULL;

    // 查找到当前语句的结束位置后，后移一个字节到下一个语句的起始位置
    const char *token = strstr(nmea_sentence, "\r\n");
    if(token) token += 2;
    else return NULL;

    // 校验起始符
    if(*token != '$') return NULL;

    return token;
}

/**
 * @brief 查找指定的 NMEA 语句
 * @param nmea_sentence 输入的 NMEA 语句字符串
 * @param header 要查找的 NMEA 语句头 e.g. "GGA", "GLL" 等
 * @return 找到的语句的起始位置指针，如果没有找到则返回 NULL
 */
const char *Nmea_FindSentence(const char *nmea_sentence, const char *header)
{
    if(!nmea_sentence || !header) return NULL;

    if(strlen(nmea_sentence) < strlen(header) + 8) return NULL; 

    // 查找起始符
    const char *token = strstr(nmea_sentence, "$");
    if(!token) return NULL;

    while(token)
    {
        // 检查 NMEA 语句头
        if(strncmp(token + 3, header, strlen(header)) == 0) break;

        token = Nmea_NextSentence(token);
    }

    if(!token) return NULL;

    // 查找是否有\r\n
    const char *end = strstr(token, "\r\n");
    if(!end) return NULL;

    return token;
}
